#include "App_Flight.h"

// PID结构体
// 俯仰角
PID_Struct pid_pitch  = {.kp = -7.0, .ki = 0, .kd = 0};
PID_Struct pid_gyro_y = {.kp = 2.0, .ki = 0, .kd = 0.3};

// 横滚角
PID_Struct pid_roll   = {.kp = -7.0, .ki = 0, .kd = 0};
PID_Struct pid_gyro_x = {.kp = -2.0, .ki = 0, .kd = 0.3};

// 偏航角
PID_Struct pid_yaw    = {.kp = -1.0, .ki = 0, .kd = 0};
PID_Struct pid_gyro_z = {.kp = -5.0, .ki = 0, .kd = 0};

// 高度
PID_Struct pid_height = {.kp = -1.2f, .ki = 0, .kd = -0.085};
PID_Struct pid_speed  = {.kp = -1.2f, .ki = 0, .kd = -0.085};

// 电机结构体
Motor_Struct motor_left_top     = {&htim3, TIM_CHANNEL_1, 0};
Motor_Struct motor_left_bottom  = {&htim4, TIM_CHANNEL_4, 0};
Motor_Struct motor_right_top    = {&htim2, TIM_CHANNEL_2, 0};
Motor_Struct motor_right_bottom = {&htim1, TIM_CHANNEL_3, 0};

// 通信任务句柄
extern TaskHandle_t communication_task_handle;
void App_Flight_SetMotorSpeed(Remote_Struct *remote_data, Flight_Status flight_status)
{
    switch (flight_status) {
        case IDEL:
            motor_left_top.speed     = 0;
            motor_left_bottom.speed  = 0;
            motor_right_top.speed    = 0;
            motor_right_bottom.speed = 0;
            break;
        case NORMAL:
            // todo:三个欧拉角的PID计算结果
            motor_left_top.speed     = LIMIT(LIMIT(remote_data->THR, 0, 800) + pid_gyro_y.output + pid_gyro_x.output + pid_gyro_z.output, 0, 1000);
            motor_left_bottom.speed  = LIMIT(LIMIT(remote_data->THR, 0, 800) - pid_gyro_y.output + pid_gyro_x.output - pid_gyro_z.output, 0, 1000);
            motor_right_top.speed    = LIMIT(LIMIT(remote_data->THR, 0, 800) + pid_gyro_y.output - pid_gyro_x.output - pid_gyro_z.output, 0, 1000);
            motor_right_bottom.speed = LIMIT(LIMIT(remote_data->THR, 0, 800) - pid_gyro_y.output - pid_gyro_x.output + pid_gyro_z.output, 0, 1000);
            break;
        case FIX_HEIGHT:
            // todo:三个欧拉角的PID计算结果+高度PID的计算结果
            motor_left_top.speed     = LIMIT(LIMIT(remote_data->THR, 0, 800) + pid_gyro_y.output + pid_gyro_x.output + pid_gyro_z.output + LIMIT(pid_speed.output, -150, 150), 0, 1000);
            motor_left_bottom.speed  = LIMIT(LIMIT(remote_data->THR, 0, 800) - pid_gyro_y.output + pid_gyro_x.output - pid_gyro_z.output + LIMIT(pid_speed.output, -150, 150), 0, 1000);
            motor_right_top.speed    = LIMIT(LIMIT(remote_data->THR, 0, 800) + pid_gyro_y.output - pid_gyro_x.output - pid_gyro_z.output + LIMIT(pid_speed.output, -150, 150), 0, 1000);
            motor_right_bottom.speed = LIMIT(LIMIT(remote_data->THR, 0, 800) - pid_gyro_y.output - pid_gyro_x.output + pid_gyro_z.output + LIMIT(pid_speed.output, -150, 150), 0, 1000);
            break;
        case FAULT:
            if (motor_left_top.speed > 0) {
                motor_left_top.speed -= 10;
                if (motor_left_top.speed < 0) {
                    motor_left_top.speed = 0;
                }
            }

            if (motor_left_bottom.speed > 0) {
                motor_left_bottom.speed -= 10;
                if (motor_left_bottom.speed < 0) {
                    motor_left_bottom.speed = 0;
                }
            }

            if (motor_right_top.speed > 0) {
                motor_right_top.speed -= 10;
                if (motor_right_top.speed < 0) {
                    motor_right_top.speed = 0;
                }
            }
            if (motor_right_bottom.speed > 0) {
                motor_right_bottom.speed -= 10;
                if (motor_right_bottom.speed < 0) {
                    motor_right_bottom.speed = 0;
                }
            }
            if ((motor_left_top.speed == 0) && (motor_left_bottom.speed == 0) && (motor_right_top.speed == 0) && (motor_right_bottom.speed == 0)) {
                xTaskNotifyGive(communication_task_handle);
            }
            break;
        default:
            break;
    }
    if (remote_data->THR < 30) {
        motor_left_top.speed     = 0;
        motor_left_bottom.speed  = 0;
        motor_right_top.speed    = 0;
        motor_right_bottom.speed = 0;
    }

    Int_Motor_SetSpeed(&motor_left_top);
    Int_Motor_SetSpeed(&motor_left_bottom);
    Int_Motor_SetSpeed(&motor_right_top);
    Int_Motor_SetSpeed(&motor_right_bottom);
}

void App_Flight_ReadAccelGyro(AccelGyro_Struct *accel_gyro)
{
    // 1.获取六轴数据
    Int_MPU6050_ReadAccelGyro(accel_gyro);

    // 2.滤波处理
    // AccelGyro_Struct unFiltered = *accel_gyro;
    // 2.1 角速度滤波处理（低通滤波）
    static Gyro_Struct last_gyro = {.x = 0, .y = 0, .z = 0};
    accel_gyro->gyro.x           = Com_Filter_LowPass(accel_gyro->gyro.x, last_gyro.x);
    accel_gyro->gyro.y           = Com_Filter_LowPass(accel_gyro->gyro.y, last_gyro.y);
    accel_gyro->gyro.z           = Com_Filter_LowPass(accel_gyro->gyro.z, last_gyro.z);
    last_gyro                    = accel_gyro->gyro;

    // 2.2 加速度滤波处理（卡尔曼滤波）
    accel_gyro->accel.x = Com_Filter_KalmanFilter(&kfs[0], accel_gyro->accel.x);
    accel_gyro->accel.y = Com_Filter_KalmanFilter(&kfs[1], accel_gyro->accel.y);
    accel_gyro->accel.z = Com_Filter_KalmanFilter(&kfs[2], accel_gyro->accel.z);

    // AccelGyro_Struct filtered = *accel_gyro;
    // printf("%d,%d\n", unFiltered.accel.x, filtered.accel.x);
}

void App_Flight_GetEulerAngle(AccelGyro_Struct *accel_gyro, EulerAngle_Struct *eulerAngle, float dt)
{
    Com_IMU_GetEulerAngle(accel_gyro, eulerAngle, dt);
}

void App_Flight_EulerAnglePID(Remote_Struct *remote_data, EulerAngle_Struct *euler_angle, AccelGyro_Struct *accel_gyro, float dt)
{
    // 1.俯仰角
    pid_pitch.desire   = (remote_data->PIT - 500) * 0.02;
    pid_pitch.measure  = euler_angle->pitch;
    pid_gyro_y.measure = accel_gyro->gyro.y * 4000.0 / 65535;
    Com_PID_Cascade(&pid_pitch, &pid_gyro_y, dt);

    // 2.横滚角
    pid_roll.desire    = (remote_data->ROL - 500) * 0.02;
    pid_roll.measure   = euler_angle->roll;
    pid_gyro_x.measure = accel_gyro->gyro.x * 4000.0 / 65535;
    Com_PID_Cascade(&pid_roll, &pid_gyro_x, dt);

    // 3.偏航角
    pid_yaw.desire     = (remote_data->YAW - 500) * 0.02;
    pid_yaw.measure    = euler_angle->yaw;
    pid_gyro_z.measure = accel_gyro->gyro.z * 4000.0 / 65535;
    Com_PID_Cascade(&pid_yaw, &pid_gyro_z, dt);
}

uint16_t last_height  = 0;
uint16_t last_speed_z = 0;
float static_accel_z  = 0;
void App_Filght_PID(uint16_t height, uint16_t fix_height, float dt)
{
    // 外环：高度
    pid_height.desire  = fix_height;
    pid_height.measure = height;

    // 内环：速度
    //  高度微分
    float speed_z_1 = (height - last_height) / dt;
    last_height     = height;
    // 加速度积分
    float speed_z_2 = last_speed_z + (Com_IMU_GetNormAccZ() - static_accel_z) * dt;

    pid_speed.measure = speed_z_2 * 0.98 + speed_z_1 * 0.02;
    last_speed_z      = pid_speed.measure;
    // PID 计算
    Com_PID_Cascade(&pid_height, &pid_speed, dt);
}